#ifndef WORLDMODEL_H
#define WORLDMODEL_H

#include <string>
#include <boost/regex.hpp>
#include <boost/shared_ptr.hpp>
#include <tsubamegaeshi-base/parser.h>

class Worldmodel :public Parser
{
public:
  enum SideID {
    S_ERROR = 0,
    S_LEFT,
    S_RIGHT,
    S_MAX
  };
  static const int HISTORY_MAX=10;

  //typedef std::map<std::string, JointID> TJointIDMap;
  //typedef std::vector<std::string> TJointNameMap;

public:
  typedef std::map<std::string, double > THJSenceMap;
  typedef std::map<std::string, std::pair<double, double> > TUJSenceMap;


  struct TVector3 {
    double v_[3];

    TVector3(double v1=0, double v2=0, double v3=0)
    {
      v_[0] = v1;
      v_[1] = v2;
      v_[2] = v3;
    }

  };
  typedef std::map<std::string, TVector3> TFlagSenceMap;
  typedef std::map<std::string, TVector3> TGyrMap;

  struct TFrpValue {
    TVector3 c_;
    TVector3 f_;

    TFrpValue(double c1=0, double c2=0, double c3=0, double f1=0, double f2=0, double f3=0)
      : c_(c1, c2, c3), f_(f1, f2, f3)
    {}
  };

  typedef std::map<std::string, TFrpValue> TFrpMap;

  struct TStatus {
    double time_;
    double gs_time_;
    std::string gs_pm_;

    THJSenceMap hj_; 
    TUJSenceMap uj_; 
    TFlagSenceMap flag_; 
    TGyrMap gyr_; 
    TFrpMap frp_;
 
    TVector3 ball_;
    TVector3 position_;
    TVector3 position_mypos_; // (mypos x y z) 
    TVector3 zax_;

    void dump();
    void calc_position();
    void clear();
  };

protected:
  const std::string teamname_;
  SideID side_;
  int number_;

  long step_;
  TStatus status_[HISTORY_MAX];
  
public:
  typedef boost::shared_ptr< Worldmodel > shared_ptr;

  Worldmodel(const std::string& teamname);
  ~Worldmodel();
  void parse(const std::string& message);

  const std::string&	teamname();
  const int		side();
  const int		number();
  //const TStatus&	current_status();
  TStatus&		current_status();
  const TStatus&	status_history(int i);

protected:
  void time_callback(const boost::match_results<std::string::const_iterator>& m, int i) ;
  void gs_callback(const boost::match_results<std::string::const_iterator>& m, int i) ;
  void gyr_callback(const boost::match_results<std::string::const_iterator>& m, int i) ;
  void flag_callback(const boost::match_results<std::string::const_iterator>& m, int i) ;
  void ball_callback(const boost::match_results<std::string::const_iterator>& m, int i) ;
  void uj_callback(const boost::match_results<std::string::const_iterator>& m, int i) ;
  void hj_callback(const boost::match_results<std::string::const_iterator>& m, int i) ;
  void frp_callback(const boost::match_results<std::string::const_iterator>& m, int i) ;
  void mypos_callback(const boost::match_results<std::string::const_iterator>& m, int i) ;

};

#endif//WORLDMODEL_H
